/*
 *
 * Copyright (C) 2014
 * Julio Jarquin, Gustavo Arechavaleta <garechav@cinvestav.edu.mx>
 * CINVESTAV - Saltillo Campus
 *
 * This file is part of HRLocomotion-1.0
 * HRLocomotion-1.0 free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * HRLocomotion-1.0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301  USA
 */

/**
 *	\file src/mpc_constraints.h
 *	\author Julio Jarquin, Gustavo Arechavaleta
 *	\version 1.0
 *	\date 2015
 *
 *	Declaration of the MPCConstraints class.
 */

#ifndef MPC_CONSTRAINTS_H
#define MPC_CONSTRAINTS_H

#include "HRLocomotion/hr_types.h"
#include "HRLocomotion/mpc_data.h"
#include "HRLocomotion/robot_state.h"
#include <vector>

namespace hrLocomotion{
/** This class can be an interface with only virtual methods, and to be implemented by other classes
 *  i.e.  NaoBounds, Hrp2Bounds, etc.
 */
class MPCConstraints
{
    typedef Eigen::Matrix<real_t,2,1> FootVector2r;

public:
    MPCConstraints();
    MPCConstraints(MPCData &mpcData);
    void mpcTest();
    /** Init the required paramters that do not depend on the current constraints, the implementation of the method depends on the robot itself */
    void init(const MPCData & mpcData);

    /** The implementation of this method depends on the robot itself */
    void setConstraints(const MPCData&,const FootStatus&,const Vector3r & footPosition, RobotState &);

    /**
    \brief This function returns constraint matrix A
      */
    MatrixXr getAMatrix(){
        return A_constraints;
    }

    /**
    \brief This function returns lower bounds vector related to the matrix A
      */
    VectorXr getblwVector(){
        return blw_constraints;
    }

    /**
    \brief This function returns upper bounds vector related to the matrix A
      */
    VectorXr getbupVector(){
        return bup_constraints;
    }

private:
    MPCData& internalMPCData;
    MatrixXr A_constraints;
    VectorXr bup_constraints;
    VectorXr blw_constraints;

    //This methods are part of the nao implementation, do not consider them when porting the class to an abstract class version

    const real_t kMaxStepX;     //Maximum translation from reference foot along the x axis  0.080m for NAO
    const real_t kMinStepX;     //Minimum translation from reference foot along the x axis -0.040m for NAO
    const real_t kMaxStepY;     //Maximum translation from reference foot along the y axis  0.160m for NAO
    const real_t kMinFootSeparation; //Minimum distance between both feet along the y axis  0.090m for NAO

    MatrixXr A_fpos;  //Constraint Matrix for the foot position.
    std::vector<MatrixXr> A_zmp;  //Constraint Matrix for the ZMP position.

    VectorXr bup_fpos;  //Upper Constraint Vector for the foot position.
    VectorXr bup_zmp;  //Upper Constraint Vector for ZMP position.

    //Foot position constraint need variables

    MatrixXr A_lfparams; //Left foot parameters matrix.       //TODO:  const matrix.
    MatrixXr A_rfparams; //Right foot parameters matrix.      //TODO:  const matrix.
    MatrixXr bbox_lfparams; //Bounding box corners for left foot  //TODO: check axis consistency
    MatrixXr bbox_rfparams; //Bounding box corners for right foot //TODO: check axis consistency

    VectorXr bup_fparams; //Foot position parameters vector. //TODO: const vector.

    //ZMP constraint needed variables
    real_t   zmp_margin;
    MatrixXr Diag_zmp;      //Diagonal matrix to form the matrix  A = Dk*[Pzu -Uk 0 0 ; 0 0 Pzu Uk]
    VectorXr bup_zparams; //Zmp position parameters vector //TODO: const vector.





};

}

#endif // MPC_CONSTRAINTS_H
